#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
麦克纳姆轮小车 ROS2 驱动 + RViz 可视化启动文件

该 launch 文件启动机器人驱动节点和 RViz2 可视化工具

使用方法:
    ros2 launch robot_chassis robot_driver_with_rviz.launch.py
"""

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取包路径
    pkg_share = get_package_share_directory('robot_chassis')
    
    # 包含机器人驱动 launch 文件
    robot_driver_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_share, 'launch', 'robot_driver.launch.py')
        ),
        launch_arguments={
            'serial_port': LaunchConfiguration('serial_port'),
            'publish_tf': 'true',
        }.items()
    )
    
    # 声明启动参数
    serial_port_arg = DeclareLaunchArgument(
        'serial_port',
        default_value='/dev/ttyACM0',
        description='串口设备路径'
    )
    
    # RViz2 节点
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', os.path.join(pkg_share, 'config', 'robot.rviz')],
        condition=None  # 总是启动
    )
    
    return LaunchDescription([
        serial_port_arg,
        robot_driver_launch,
        rviz_node,
    ])

